using System;
using Microsoft.SPOT;

namespace MLRobotic.IA.Goals
{
    public class WaitUsClear : Goal
    {
        private const int MaxWaitInMilliseconds = 2000;
        private int _distance;
        private DateTime _waitStop;

        public WaitUsClear(int distance)
            : base()
        {
            _distance = distance;
        }
    
        public override void Process()
        {
            if (State == GoalSate.Todo)
            {
                State = GoalSate.Doing;
                Com.Stop();
                _waitStop = DateTime.Now.AddMilliseconds(MaxWaitInMilliseconds);
            }
            else if (State == GoalSate.Doing)
            {
                if (Robot.UsDistance > _distance)
                {
                    State = GoalSate.Done;
                }
                else if (DateTime.Now > _waitStop)
                {
                    Timeout = true;
                    State = GoalSate.Done;
                }
            }
        }
    }
}
